#include <error.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include <ros/duration.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Int32.h>
#include <std_msgs/UInt32.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_datatypes.h>

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <deque>
#include <thread>
#include <time.h>
#include <unistd.h>
#include <chrono>
#include <iomanip>
#include <iostream>
#include <netinet/in.h>

#include <Eigen/Eigen>
#include <Eigen/Core>

#include "X20_LegKinematic.hpp"


Eigen::Vector3d LF_Angle;   // 左前角度
Eigen::Vector3d LF_Vel;     // 左前角速度
Eigen::Vector3d LF_Tor;     // 左前力矩

int X20_Control_Mode;   // X20控制模式
int X20_Basic_State;    // X20状态
int X20_Gait_State;     // X20步态
int X20_Task_state;     // X20任务状态

bool Angle_Rvise = false;   // 修正角度数据，在踏步时的角度数据有偏差


X20LegKinematic X20Leg;


#define SERV_PORT 43897     // 服务端口
#define PI 3.1415926        // pi

/* If use QNX host, please uncomment the following line */
// #define USE_QNX_HOST
#ifdef USE_QNX_HOST
#pragma pack(4)
#endif
struct RobotState {
  double rpy[3];
  double rpy_vel[3];
  double xyz_acc[3];
  double pos_world[3];
  double vel_world[3];
  unsigned touch_down_and_stair_trot;
  bool is_charging;
  unsigned error_state;
  int control_mode;
  double battery_volt;
  double driver_temp;
  double motor_temp;
  int robot_basic_state;
  int robot_gait_state;
  int robot_task_state;
};
#pragma pack(4)


// 收到的数据帧头，用于判断接收数据的类型
struct Command
{
  int code;
  int size;
  int cons_code;
};

// 机器人接收的状态数据
struct RobotStateReceived {
  int code;
  int size;
  int cons_code;
  struct RobotState data;
};


// 关节角度
struct JointState {
  double LF_Joint;
  double LF_Joint_1;
  double LF_Joint_2;
  double RF_Joint;
  double RF_Joint_1;
  double RF_Joint_2;
  double LB_Joint;
  double LB_Joint_1;
  double LB_Joint_2;
  double RB_Joint;
  double RB_Joint_1;
  double RB_Joint_2;
};
struct JointStateReceived {
  int code;
  int size;
  int cons_code;
  struct JointState data;
};


// 关节角速度
struct JointVel {
  double LF_Joint;
  double LF_Joint_1;
  double LF_Joint_2;
  double RF_Joint;
  double RF_Joint_1;
  double RF_Joint_2;
  double LB_Joint;
  double LB_Joint_1;
  double LB_Joint_2;
  double RB_Joint;
  double RB_Joint_1;
  double RB_Joint_2;
};
struct JointVelReceived {
  int code;
  int size;
  int cons_code;
  struct JointVel data;
};


// 关节力矩
struct JointTorque {
  double LF_Joint;
  double LF_Joint_1;
  double LF_Joint_2;
  double RF_Joint;
  double RF_Joint_1;
  double RF_Joint_2;
  double LB_Joint;
  double LB_Joint_1;
  double LB_Joint_2;
  double RB_Joint;
  double RB_Joint_1;
  double RB_Joint_2;
};
struct JointTorqueReceived {
  int code;
  int size;
  int cons_code;
  struct JointTorque data;
};




struct gaitStateReceived {
  int code;
  int size;
  int cons_code;
};

// 手柄状态数据
struct handleState {
  double left_axis_forward;
  double left_axis_side;
  double right_axis_yaw;
  double goal_vel_forward;
  double goal_vel_side;
  double goal_vel_yaw;
};
struct handleStateReceived {
  int code;
  int size;
  int cons_code;
  struct handleState data;
};



class X20dataReceiver
{
public:
    
    ros::NodeHandle nh;
    ros::Publisher pub_X20_leg_odom;        // 里程计发布器
    ros::Publisher pub_X20_joint_angle;     // 关节角度发布器
    ros::Publisher pub_X20_joint_vel;       // 关节角速度发布器
    ros::Publisher pub_X20_joint_tor;       // 关节力矩发布器
    ros::Publisher pub_X20_joint_State;

    double X20_joint_angle[12];   // 记录所有关节电机的角度数组
    double X20_joint_vel[12];     // 记录所有关节电机的角速度数组
    double X20_joint_tor[12];     // 记录所有关节电机的力矩数组

    bool use_vel_world = true;
    int code_id = 2305;         // 数据帧头的编码：(2305-odom; 2306-angle; 2307-vel; 2308-tor)
    int port = 43897;           // 数据端口


    X20dataReceiver()
    {   
        pub_X20_leg_odom = nh.advertise<nav_msgs::Odometry>("leg_odom", 1);
        pub_X20_joint_angle = nh.advertise<sensor_msgs::JointState>("X20_joint_angle", 1);
        pub_X20_joint_vel = nh.advertise<sensor_msgs::JointState>("X20_joint_vel", 1);
        pub_X20_joint_tor = nh.advertise<sensor_msgs::JointState>("X20_joint_tor", 1);
        pub_X20_joint_State = nh.advertise<sensor_msgs::JointState>("joint_states", 1);
    }


    /************************************************************************
     * @brief 发布X20关节电机的所有数据话题
     * 
     * 数据包含：电机角度、电机角速度、电机力矩
     * 
     ***********************************************************************/
    void pub_X20_JointTotalData()
    {
        sensor_msgs::JointState joint_data;
        joint_data.header.stamp = ros::Time::now();
        joint_data.name.resize(12);
        joint_data.position.resize(12);     // 重置电机角度数据
        joint_data.velocity.resize(12);     // 重置电机角速度数据
        joint_data.effort.resize(12);       // 重置电机力矩数据

        // X20左前腿关节电机数据
        joint_data.name[0] = "LF_Joint";
        joint_data.position[0] = X20_joint_angle[0];
        joint_data.velocity[0] = X20_joint_vel[0];
        joint_data.effort[0] = X20_joint_tor[0];
        joint_data.name[1] = "LF_Joint_1";
        joint_data.position[1] = X20_joint_angle[1];
        joint_data.velocity[1] = X20_joint_vel[1];
        joint_data.effort[1] = X20_joint_tor[1];
        joint_data.name[2] = "LF_Joint_2";
        joint_data.position[2] = X20_joint_angle[2];
        joint_data.velocity[2] = X20_joint_vel[2];
        joint_data.effort[2] = X20_joint_tor[2];

        // X20右前腿关节电机数据
        joint_data.name[3] = "RF_Joint";
        joint_data.position[3] = X20_joint_angle[3];
        joint_data.velocity[3] = X20_joint_vel[3];
        joint_data.effort[3] = X20_joint_tor[3];
        joint_data.name[4] = "RF_Joint_1";
        joint_data.position[4] = X20_joint_angle[4];
        joint_data.velocity[4] = X20_joint_vel[4];
        joint_data.effort[4] = X20_joint_tor[4];
        joint_data.name[5] = "RF_Joint_2";
        joint_data.position[5] = X20_joint_angle[5];
        joint_data.velocity[5] = X20_joint_vel[5];
        joint_data.effort[5] = X20_joint_tor[5];

        // X20左后腿关节电机数据
        joint_data.name[6] = "LB_Joint";
        joint_data.position[6] = X20_joint_angle[6];
        joint_data.velocity[6] = X20_joint_vel[6];
        joint_data.effort[6] = X20_joint_tor[6];
        joint_data.name[7] = "LB_Joint_1";
        joint_data.position[7] = X20_joint_angle[7];
        joint_data.velocity[7] = X20_joint_vel[7];
        joint_data.effort[7] = X20_joint_tor[7];
        joint_data.name[8] = "LB_Joint_2";
        joint_data.position[8] = X20_joint_angle[8];
        joint_data.velocity[8] = X20_joint_vel[8];
        joint_data.effort[8] = X20_joint_tor[8];

        // X20右后腿关节电机数据
        joint_data.name[9] = "RB_Joint";
        joint_data.position[9] = X20_joint_angle[9];
        joint_data.velocity[9] = X20_joint_vel[9];
        joint_data.effort[9] = X20_joint_tor[9];
        joint_data.name[10] = "RB_Joint_1";
        joint_data.position[10] = X20_joint_angle[10];
        joint_data.velocity[10] = X20_joint_vel[10];
        joint_data.effort[10] = X20_joint_tor[10];
        joint_data.name[11] = "RB_Joint_2";
        joint_data.position[11] = X20_joint_angle[11];
        joint_data.velocity[11] = X20_joint_vel[11];
        joint_data.effort[11] = X20_joint_tor[11];

        pub_X20_joint_State.publish(joint_data);
    }
    
    
    /************************************************************************
     * @brief 处理X20发送的UDP数据帧
     * 
     * 封装数据话题：
     * 1.里程计话题：    "leg_odom"          --- <nav_msgs::Odometry> 
     * 2.关节角度话题：  "X20_joint_angle"   --- <sensor_msgs::JointState>
     * 3.关节角速度话题："X20_joint_vel"     --- <sensor_msgs::JointState>
     * 4.关节力矩话题：  "X20_joint_tor"     --- <sensor_msgs::JointState>
     * 5.电机所有数据：  "joint_states"      --- <sensor_msgs::JointState>
     ************************************************************************/
    void processX20dataReceiver()
    {
        int sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
        if (sock_fd < 0) {
            perror("socket");
            exit(1);
        }
        
        // initial socket
        struct sockaddr_in addr_serv;
        int len;

        memset(&addr_serv, 0, sizeof(struct sockaddr_in));
        addr_serv.sin_family = AF_INET;     // 指定IP地址地址版本为IPV4
        addr_serv.sin_port = htons(port);   // 指定连接的目标端口号
        addr_serv.sin_addr.s_addr = htonl(INADDR_ANY);  // 接受来自任意IP地址的连接，INADDR_ANY真实值为0.0.0.0
        len = sizeof(addr_serv);
        // 使用bind() 函数绑定监听端口，将套接字文件描述符sock_fd和地址类型变量（struct sockaddr_in ）进行绑定(0: 绑定成功; -1: 绑定失败)
        if (bind(sock_fd, (struct sockaddr *)&addr_serv, sizeof(addr_serv)) < 0) {
            perror("bind error:");
            exit(1);
        }
        int recv_num = -1;
        char recv_buf[2048];    // 接收的客户端网络数据缓存数组
        struct sockaddr_in addr_client;
        
        std::chrono::steady_clock::time_point start_outer = std::chrono::steady_clock::now();
        int64_t counter_RobotState = 0;
        int64_t counter_JointState = 0;
        int64_t counter_sum = 0;
        ros::Rate loop_rate(1000);

        while (ros::ok())
        {
            // 使用recvfrom() 函数接收客户端的网络数据（成功接收，则返回接收到的字节数）
            if ((recv_num = recvfrom(sock_fd, recv_buf, sizeof(recv_buf), 0,
                                    (struct sockaddr *)&addr_client,
                                    (socklen_t *)&len)) < 0) 
            {
                perror("recvfrom error:");
                exit(1);
            }
            counter_sum++;

            std::chrono::duration<double> time_counter =
                std::chrono::duration_cast<std::chrono::duration<double>>(
                    std::chrono::steady_clock::now() - start_outer);

            if (time_counter.count() >= 1.0) 
            {
                counter_RobotState = 0;
                counter_JointState = 0;
                counter_sum = 0;
                start_outer = std::chrono::steady_clock::now();
            }
  
            // 取出接收到的数据帧头，用于判断是哪个数据，然后进行解析
            Command command;
            memset(&command, 0, sizeof(Command));
            memcpy(&command, recv_buf, sizeof(Command));

            // 根据帧头的code编码，进行数据对应格式的解析
            if (1)
            {
                /***********************************************************************
                 * 通过数据Command类型的帧头信息，用于判断接收到哪个数据，然后进行解析
                 * command.code:
                 * 2305: RobotState   机器人状态数据
                 * 2306: JointState   关节电机角度状态数据
                 * 2307: JointVel     关节电机角速度状态数据
                 * 2308: JointTorque  关节电机力矩状态数据
                 * 2309: handleState  手柄状态数据
                 ***********************************************************************/
                if(command.code == code_id)
                {
                    RobotState robotstate;
                    memset(&robotstate, 0, sizeof(RobotState));
                    memcpy(&robotstate, recv_buf + sizeof(Command), command.size);
                    RobotState *robot_state = &robotstate;

                    nav_msgs::Odometry leg_odom_data;
                    leg_odom_data.header.frame_id = "odom";
                    leg_odom_data.child_frame_id = "base_link";

                    // Position
                    leg_odom_data.header.stamp = ros::Time::now();
                    auto q = tf::createQuaternionFromRPY(robot_state->rpy[0] / 180 * PI,
                                                        robot_state->rpy[1] / 180 * PI,
                                                        robot_state->rpy[2] / 180 * PI);
                    tf::quaternionTFToMsg(q, leg_odom_data.pose.pose.orientation);
                    leg_odom_data.pose.pose.position.x = robot_state->pos_world[0];
                    leg_odom_data.pose.pose.position.y = robot_state->pos_world[1];
                    leg_odom_data.pose.pose.position.z = robot_state->pos_world[2];

                    // Velocity
                    double yaw = robot_state->rpy[2] / 180 * PI;

                    if (use_vel_world)  // vel_world to vel_base
                    {  
                    leg_odom_data.twist.twist.linear.x =
                            +robot_state->vel_world[0] * cos(yaw) +
                            robot_state->vel_world[1] * sin(yaw);
                    leg_odom_data.twist.twist.linear.y =
                            -robot_state->vel_world[0] * sin(yaw) +
                            robot_state->vel_world[1] * cos(yaw);
                    }
                    else    // vel_base
                    {  
                        leg_odom_data.twist.twist.linear.x = robot_state->vel_world[0];
                        leg_odom_data.twist.twist.linear.y = robot_state->vel_world[1];
                    }

                    leg_odom_data.twist.twist.angular.z = robot_state->rpy_vel[2];
                    pub_X20_leg_odom.publish(leg_odom_data);

                    // X20的控制模式：0:手动模式; 1:导航模式;
                    X20_Control_Mode = robot_state->control_mode;
                    // X20的状态：0:趴下; 1:正在起立; 2:初始站立; 3:力控站立; 4:踏步; 5:正在趴下; 6:软急停;
                    X20_Basic_State = robot_state->robot_basic_state;
                    // X20的步态：0:正常; 1:楼梯; 2:斜坡; 3:跑步; 5:匍匐;
                    X20_Gait_State = robot_state->robot_gait_state;
                    // X20任务状态
                    X20_Task_state = robot_state->robot_task_state;

                    // X20电机角度修正标志位
                    if (X20_Basic_State == 0 || X20_Basic_State == 1 || X20_Basic_State == 5)
                    {
                        Angle_Rvise = false;
                    }
                    else if (X20_Basic_State == 4)
                    {
                        Angle_Rvise = true;
                    }


                    counter_RobotState++;
                }
                else if(command.code == code_id + 1)  // 关节角度信息
                {
                    JointState jointstate;
                    memset(&jointstate, 0, sizeof(JointState));
                    memcpy(&jointstate, recv_buf + sizeof(Command), command.size);
                    JointState *joint_state = &jointstate;

                    sensor_msgs::JointState joint_state_data;
                    joint_state_data.header.stamp = ros::Time::now();
                    joint_state_data.name.resize(12);
                    joint_state_data.position.resize(12);

                    joint_state_data.name[0] = "LF_Joint";
                    joint_state_data.position[0] = joint_state->LF_Joint;
                    joint_state_data.name[1] = "LF_Joint_1";
                    joint_state_data.position[1] = joint_state->LF_Joint_1;
                    joint_state_data.name[2] = "LF_Joint_2";
                    joint_state_data.position[2] = joint_state->LF_Joint_2;

                    joint_state_data.name[3] = "RF_Joint";
                    joint_state_data.position[3] = joint_state->RF_Joint;
                    joint_state_data.name[4] = "RF_Joint_1";
                    joint_state_data.position[4] = joint_state->RF_Joint_1;
                    joint_state_data.name[5] = "RF_Joint_2";
                    joint_state_data.position[5] = joint_state->RF_Joint_2;

                    joint_state_data.name[6] = "LB_Joint";
                    joint_state_data.position[6] = joint_state->LB_Joint;
                    joint_state_data.name[7] = "LB_Joint_1";
                    joint_state_data.position[7] = joint_state->LB_Joint_1;
                    joint_state_data.name[8] = "LB_Joint_2";
                    joint_state_data.position[8] = joint_state->LB_Joint_2;

                    joint_state_data.name[9] = "RB_Joint";
                    joint_state_data.position[9] = joint_state->RB_Joint;
                    joint_state_data.name[10] = "RB_Joint_1";
                    joint_state_data.position[10] = joint_state->RB_Joint_1;
                    joint_state_data.name[11] = "RB_Joint_2";
                    joint_state_data.position[11] = joint_state->RB_Joint_2;
                    pub_X20_joint_angle.publish(joint_state_data);
                    counter_JointState++;

                    /*********************** 测试代码段 **************************/
                    // 记录LF三个关节的角度数据
                    // LF_Angle(0) = joint_state->LF_Joint;
                    // LF_Angle(1) = joint_state->LF_Joint_1;
                    // LF_Angle(2) = joint_state->LF_Joint_2;


                    /*********************** 数据记录代码段 **************************/
                    if (Angle_Rvise)
                    {
                        X20_joint_angle[0] = joint_state->LF_Joint - 0.154;
                        X20_joint_angle[1] = joint_state->LF_Joint_1;
                        X20_joint_angle[2] = joint_state->LF_Joint_2;

                        X20_joint_angle[3] = joint_state->RF_Joint + 0.167;
                        X20_joint_angle[4] = joint_state->RF_Joint_1;
                        X20_joint_angle[5] = joint_state->RF_Joint_2;

                        X20_joint_angle[6] = joint_state->LB_Joint - 0.192;
                        X20_joint_angle[7] = joint_state->LB_Joint_1;
                        X20_joint_angle[8] = joint_state->LB_Joint_2;

                        X20_joint_angle[9] = joint_state->RB_Joint + 0.192;
                        X20_joint_angle[10] = joint_state->RB_Joint_1;
                        X20_joint_angle[11] = joint_state->RB_Joint_2;
                    }
                    else
                    {
                        X20_joint_angle[0] = joint_state->LF_Joint;
                        X20_joint_angle[1] = joint_state->LF_Joint_1;
                        X20_joint_angle[2] = joint_state->LF_Joint_2;

                        X20_joint_angle[3] = joint_state->RF_Joint;
                        X20_joint_angle[4] = joint_state->RF_Joint_1;
                        X20_joint_angle[5] = joint_state->RF_Joint_2;

                        X20_joint_angle[6] = joint_state->LB_Joint;
                        X20_joint_angle[7] = joint_state->LB_Joint_1;
                        X20_joint_angle[8] = joint_state->LB_Joint_2;

                        X20_joint_angle[9] = joint_state->RB_Joint;
                        X20_joint_angle[10] = joint_state->RB_Joint_1;
                        X20_joint_angle[11] = joint_state->RB_Joint_2;
                    }


                    
                }
                else if(command.code == code_id + 2)  // 关节角速度信息
                {
                    JointVel jointvel;
                    memset(&jointvel, 0, sizeof(JointVel));
                    memcpy(&jointvel, recv_buf + sizeof(Command), command.size);
                    JointVel *joint_vel = &jointvel;

                    sensor_msgs::JointState joint_vel_data;
                    joint_vel_data.header.stamp = ros::Time::now();
                    joint_vel_data.name.resize(12);
                    joint_vel_data.position.resize(12);

                    joint_vel_data.name[0] = "LF_Joint";
                    joint_vel_data.position[0] = joint_vel->LF_Joint;
                    joint_vel_data.name[1] = "LF_Joint_1";
                    joint_vel_data.position[1] = joint_vel->LF_Joint_1;
                    joint_vel_data.name[2] = "LF_Joint_2";
                    joint_vel_data.position[2] = joint_vel->LF_Joint_2;

                    joint_vel_data.name[3] = "RF_Joint";
                    joint_vel_data.position[3] = joint_vel->RF_Joint;
                    joint_vel_data.name[4] = "RF_Joint_1";
                    joint_vel_data.position[4] = joint_vel->RF_Joint_1;
                    joint_vel_data.name[5] = "RF_Joint_2";
                    joint_vel_data.position[5] = joint_vel->RF_Joint_2;

                    joint_vel_data.name[6] = "LB_Joint";
                    joint_vel_data.position[6] = joint_vel->LB_Joint;
                    joint_vel_data.name[7] = "LB_Joint_1";
                    joint_vel_data.position[7] = joint_vel->LB_Joint_1;
                    joint_vel_data.name[8] = "LB_Joint_2";
                    joint_vel_data.position[8] = joint_vel->LB_Joint_2;

                    joint_vel_data.name[9] = "RB_Joint";
                    joint_vel_data.position[9] = joint_vel->RB_Joint;
                    joint_vel_data.name[10] = "RB_Joint_1";
                    joint_vel_data.position[10] = joint_vel->RB_Joint_1;
                    joint_vel_data.name[11] = "RB_Joint_2";
                    joint_vel_data.position[11] = joint_vel->RB_Joint_2;
                    pub_X20_joint_vel.publish(joint_vel_data);

                    /*********************** 数据记录代码段 **************************/
                    X20_joint_vel[0] = joint_vel->LF_Joint;
                    X20_joint_vel[1] = joint_vel->LF_Joint_1;
                    X20_joint_vel[2] = joint_vel->LF_Joint_2;

                    X20_joint_vel[3] = joint_vel->RF_Joint;
                    X20_joint_vel[4] = joint_vel->RF_Joint_1;
                    X20_joint_vel[5] = joint_vel->RF_Joint_2;

                    X20_joint_vel[6] = joint_vel->LB_Joint;
                    X20_joint_vel[7] = joint_vel->LB_Joint_1;
                    X20_joint_vel[8] = joint_vel->LB_Joint_2;

                    X20_joint_vel[9] = joint_vel->RB_Joint;
                    X20_joint_vel[10] = joint_vel->RB_Joint_1;
                    X20_joint_vel[11] = joint_vel->RB_Joint_2;
                }
                else if(command.code == code_id + 3)  // 关节力矩信息
                {
                    JointTorque jointtorque;
                    memset(&jointtorque, 0, sizeof(JointTorque));
                    memcpy(&jointtorque, recv_buf + sizeof(Command), command.size);
                    JointTorque *joint_torque = &jointtorque;

                    sensor_msgs::JointState joint_tor_data;
                    joint_tor_data.header.stamp = ros::Time::now();
                    joint_tor_data.name.resize(12);
                    joint_tor_data.position.resize(12);

                    joint_tor_data.name[0] = "LF_Joint";
                    joint_tor_data.position[0] = joint_torque->LF_Joint;
                    joint_tor_data.name[1] = "LF_Joint_1";
                    joint_tor_data.position[1] = joint_torque->LF_Joint_1;
                    joint_tor_data.name[2] = "LF_Joint_2";
                    joint_tor_data.position[2] = joint_torque->LF_Joint_2;

                    joint_tor_data.name[3] = "RF_Joint";
                    joint_tor_data.position[3] = joint_torque->RF_Joint;
                    joint_tor_data.name[4] = "RF_Joint_1";
                    joint_tor_data.position[4] = joint_torque->RF_Joint_1;
                    joint_tor_data.name[5] = "RF_Joint_2";
                    joint_tor_data.position[5] = joint_torque->RF_Joint_2;

                    joint_tor_data.name[6] = "LB_Joint";
                    joint_tor_data.position[6] = joint_torque->LB_Joint;
                    joint_tor_data.name[7] = "LB_Joint_1";
                    joint_tor_data.position[7] = joint_torque->LB_Joint_1;
                    joint_tor_data.name[8] = "LB_Joint_2";
                    joint_tor_data.position[8] = joint_torque->LB_Joint_2;

                    joint_tor_data.name[9] = "RB_Joint";
                    joint_tor_data.position[9] = joint_torque->RB_Joint;
                    joint_tor_data.name[10] = "RB_Joint_1";
                    joint_tor_data.position[10] = joint_torque->RB_Joint_1;
                    joint_tor_data.name[11] = "RB_Joint_2";
                    joint_tor_data.position[11] = joint_torque->RB_Joint_2;
                    pub_X20_joint_tor.publish(joint_tor_data);
                    

                    /*********************** 数据记录代码段 **************************/
                    X20_joint_tor[0] = joint_torque->LF_Joint;
                    X20_joint_tor[1] = joint_torque->LF_Joint_1;
                    X20_joint_tor[2] = joint_torque->LF_Joint_2;

                    X20_joint_tor[3] = joint_torque->RF_Joint;
                    X20_joint_tor[4] = joint_torque->RF_Joint_1;
                    X20_joint_tor[5] = joint_torque->RF_Joint_2;

                    X20_joint_tor[6] = joint_torque->LB_Joint;
                    X20_joint_tor[7] = joint_torque->LB_Joint_1;
                    X20_joint_tor[8] = joint_torque->LB_Joint_2;

                    X20_joint_tor[9] = joint_torque->RB_Joint;
                    X20_joint_tor[10] = joint_torque->RB_Joint_1;
                    X20_joint_tor[11] = joint_torque->RB_Joint_2;

                    pub_X20_JointTotalData();

                    
                    /*********************** 单腿测试代码段 **************************/
                    // LF_Tor(0) = joint_torque->LF_Joint;
                    // LF_Tor(1) = joint_torque->LF_Joint_1;
                    // LF_Tor(2) = joint_torque->LF_Joint_2;

                    // Eigen::Vector3d LFFroce;
                    // LFFroce = X20Leg.calcFoc(LF_Angle, LF_Tor);

                    // 打印输出足端力矩的x, y, z 的信息
                    // ROS_INFO("\033[1;32m %f; \033[0m", LFFroce(0));
                    // ROS_INFO("\033[1;32m %f; \033[0m", LFFroce(1));
                    // ROS_INFO("\033[1;32m %f; \033[0m", LFFroce(2));
                    // ROS_INFO("\033[1;31m ------------- \033[0m");

                    // 计算LF的足端到髋关节的高度信息
                    // Eigen::Vector3d pEe2H;
                    // pEe2H = X20Leg.calcEe2H(LF_Angle);

                    // 打印输出足端高度的x, y, z 的信息
                    // ROS_INFO("\033[1;34m %f; \033[0m", pEe2H(0));
                    // ROS_INFO("\033[1;34m %f; \033[0m", pEe2H(1));
                    // ROS_INFO("\033[1;34m %f; \033[0m", pEe2H(2));
                    // ROS_INFO("\033[1;31m ------------- \033[0m");


                    
                    
                    /******** 测试X20_LegKinematic.hpp的接口代码段 ********/
                    // Eigen::Matrix<double, 3, 4> Joint_Angel;
                    // Eigen::Matrix<double, 3, 4> Joint_Angel_Vel;
                    // Eigen::Matrix<double, 3, 4> Joint_Torque;

                    // for (int i = 0; i < 4; i++)
                    // {
                    //     Joint_Angel(0, i) = X20_joint_angle[i * 3];
                    //     Joint_Angel(1, i) = X20_joint_angle[i * 3 + 1];
                    //     Joint_Angel(2, i) = X20_joint_angle[i * 3 + 2];

                    //     Joint_Angel_Vel(0, i) = X20_joint_vel[i * 3];
                    //     Joint_Angel_Vel(1, i) = X20_joint_vel[i * 3 + 1];
                    //     Joint_Angel_Vel(2, i) = X20_joint_vel[i * 3 + 2];

                    //     Joint_Torque(0, i) = X20_joint_tor[i * 3];
                    //     Joint_Torque(1, i) = X20_joint_tor[i * 3 + 1];
                    //     Joint_Torque(2, i) = X20_joint_tor[i * 3 + 2];
                    // }

                    // X20Leg.foot_angle = Joint_Angel;
                    // X20Leg.foot_angle_vel = Joint_Angel_Vel;
                    // X20Leg.foot_torque = Joint_Torque;

                    // X20Leg.calculateFootState_Body();

                    /********************** 打印测试电机角度数据 ***********************/                
                    // ROS_INFO("\033[1;33m %f; \033[0m", X20Leg.foot_pos_body(0, 0));
                    // ROS_INFO("\033[1;33m %f; \033[0m", X20Leg.foot_pos_body(0, 1));
                    // ROS_INFO("\033[1;33m %f; \033[0m", X20Leg.foot_pos_body(0, 2));
                    // ROS_INFO("\033[1;33m %f; \033[0m", X20Leg.foot_pos_body(0, 3));
                    // ROS_INFO("\033[1;33m ------------- \033[0m");

                    // ROS_INFO("\033[1;35m %f; \033[0m", X20Leg.foot_pos_body(1, 0));
                    // ROS_INFO("\033[1;35m %f; \033[0m", X20Leg.foot_pos_body(1, 1));
                    // ROS_INFO("\033[1;35m %f; \033[0m", X20Leg.foot_pos_body(1, 2));
                    // ROS_INFO("\033[1;35m %f; \033[0m", X20Leg.foot_pos_body(1, 3));
                    // ROS_INFO("\033[1;35m ------------- \033[0m");

                    // ROS_INFO("\033[1;34m %f; \033[0m", X20Leg.foot_pos_body(2, 0));
                    // ROS_INFO("\033[1;34m %f; \033[0m", X20Leg.foot_pos_body(2, 1));
                    // ROS_INFO("\033[1;34m %f; \033[0m", X20Leg.foot_pos_body(2, 2));
                    // ROS_INFO("\033[1;34m %f; \033[0m", X20Leg.foot_pos_body(2, 3));
                    // ROS_INFO("\033[1;31m ------------- \033[0m");

                    // ROS_INFO("\033[1;33m X20 Control Model is : %d; \033[0m", X20_Control_Mode);
                    // ROS_INFO("\033[1;33m X20 Basic State is : %d; \033[0m", X20_Basic_State);
                    // ROS_INFO("\033[1;33m X20 Gait State is : %d; \033[0m", X20_Gait_State);
                    // ROS_INFO("\033[1;33m X20 Task state is : %d; \033[0m", X20_Task_state);
                    // ROS_INFO("\033[1;33m ------------- \033[0m");

                    // ROS_INFO("\033[1;31m %f; \033[0m", X20Leg.foot_contact(0));
                    // ROS_INFO("\033[1;31m %f; \033[0m", X20Leg.foot_contact(1));
                    // ROS_INFO("\033[1;31m %f; \033[0m", X20Leg.foot_contact(2));
                    // ROS_INFO("\033[1;31m %f; \033[0m", X20Leg.foot_contact(3));
                    // ROS_INFO("\033[1;33m ------------- \033[0m");

                }
            }

            loop_rate.sleep();
        }

        close(sock_fd);
    }


};




int main(int argc, char** argv)
{
    ros::init(argc, argv, "X20dataReceiver");
    
    X20dataReceiver MO;

    ROS_INFO("\033[1;32m----> X20dataReceiver Procee Started.\033[0m");

    std::thread X20dataReceiverThread(&X20dataReceiver::processX20dataReceiver, &MO);

    X20dataReceiverThread.join();

    return 0;
}